#!/bin/bash

# 获取当前工作目录
WORKSPACE_DIR=$(pwd)

echo "启动SLAM系统..."

# 第一个终端：启动SLAM launch文件
gnome-terminal --title="SLAM Launch" -- bash -c "
    cd $WORKSPACE_DIR
    source install/setup.bash
    ros2 launch robot_launch slam.launch.py
    exec bash
"

# 等待一下让第一个终端启动
sleep 2

# 第二个终端：启动语音规划器动作服务器
gnome-terminal --title="Voice Planner" -- bash -c "
    cd $WORKSPACE_DIR
    source install/setup.bash
    ros2 run voice_planner voice_planner_action_server
    exec bash
"

# 等待一下让第二个终端启动
sleep 2

# 第三个终端：启动键盘遥控
gnome-terminal --title="Teleop Keyboard" -- bash -c "
    cd $WORKSPACE_DIR
    ros2 run teleop_twist_keyboard teleop_twist_keyboard
    exec bash
"

# 等待一下让第三个终端启动
sleep 2

# 第四个终端：启动Cartographer
gnome-terminal --title="Cartographer" -- bash -c "
    cd $WORKSPACE_DIR
    source install/setup.bash
    ros2 launch fruit_cartographer cartographer.launch.py
    exec bash
"

echo "所有终端已启动完成！"
echo "- 终端1: SLAM Launch"
echo "- 终端2: Voice Planner Action Server"  
echo "- 终端3: Teleop Twist Keyboard"
echo "- 终端4: Cartographer"
